#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     motorH,        tmotorNone, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     motorI,        tmotorNone, openLoop)
#pragma config(Servo,  srvo_S2_C2_1,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "joystickdriver.c"

#define rightjoy joystick.joy1_y2/127.0*100.0
#define leftjoy joystick.joy1_y1/127.0*100.0

task main()
{
  bool slow=false;
  bool flipped=false;
  while(true)
  {
    getJoystickSettings(joystick);
    if(joy1Btn(7)&&!flipped)
    {
      flipped=true;
      slow=!slow;
    }
    if(!joy1Btn(7))
    {
      flipped=false;
    }
    if(!slow)
    {
      if (abs(leftjoy)>10)
      {
        motor[motorD]=leftjoy;
        motor[motorF]=leftjoy;
      }
      else
      {
        motor[motorD]=0;
        motor[motorF]=0;
      }
      if (abs(rightjoy)>10)
      {
        motor[motorG]=rightjoy;
        motor[motorE]=rightjoy;
      }
      else
      {
        motor[motorG]=0;
        motor[motorE]=0;
      }
    }
    //slow
    else
    {
      if (abs(leftjoy)>10)
      {
        motor[motorD]=leftjoy/3;
        motor[motorF]=leftjoy/3;
      }
      else
      {
        motor[motorD]=0;
        motor[motorF]=0;
      }
      if (abs(rightjoy)>10)
      {
        motor[motorG]=rightjoy/3;
        motor[motorE]=rightjoy/3;
      }
      else
      {
        motor[motorG]=0;
        motor[motorE]=0;
      }
    }
  }
}
